#ifndef PHIDGET_ALREADY
#define PHIDGET_ALREADY
#include "phidget21.h"
#endif

#define SHOULDER_VELOCITY X			// FILL IN THESE
#define SHOULDER_ACCELERATION Y		// FILL IN THESE


class ShoulderController {

public:
	CPhidgetMotorControlHandle motor;

	void init(CPhidgetMotorControlHandle motorIn) {
		motor = motorIn;
		
		//
		//	MOTOR MAX VELOCITY, ACCELERATION, ETC SET HERE
		//

		CPhidgetMotorControl_setVelocity(motor, 0, 0.0);
	}

	// engage is not really defined for DC motor controller
	void engage() {
		CPhidgetMotorControl_setVelocity(motor, 0, 0.0);
	} 

	void setCurrentAsHome() {
		CPhidgetMotorControl_setEncoderPosition(motor, 0, 0.0); // sets current encoder position as 0.0 on
	}

	void setVelocity(double vel) {
		CPhidgetMotorControl_setVelocity(motor, 0, vel);
	}
	
	void pidControl() {
		// DO SOMETHING LIKE THIS??
	}

	// disengage is not really defined for DC motor controller
	void disengage() {
		CPhidgetMotorControl_setVelocity(motor, 0, 0.0);
	}

};